/******************************* Includes *****************************/

#include <TimerOne.h>
#include <led_matrix.h>
#include <fix_fft.h>
#include <avr/pgmspace.h>
#include <stdint.h>

/******************************* Constants **************************/

/* Window size in samples */
#define N                       (128)
#define LOG_2_N          (7)

#define SAMPLING_RATE       (4000)

#define ANALOG_PIN_OFFSET	(14)
#define MIKE_PIN   			(2)     /* analog input - pin 14 */
#define BUILTIN_LED_PIN          (13)

#define USEC_IN_SEC		(1000000)

#define ROWS                         (3)
#define COLUMNS                  (5)
#define LED_PINS_NUM      (ROWS * COLUMNS)

#define BANDS_NUM		     (COLUMNS)

static const unsigned int BAND_PEAKS[BANDS_NUM] = {
	500,
	500,
	500,
	600,
	600
}; /* end of BAND_THRESHOLDS */

static const unsigned int BAND_THRESHOLDS[BANDS_NUM] = {
	100,
	100,
	100,
	100,
	100
};

static const unsigned int BAND_RANGE[BANDS_NUM][2] = {
	{1, 5},
	{6, 10},
	{11, 15},
	{16, 23},
	{24, 30}
}; /* end of BAND_RANGE */

/* Mapping of rows and columns to Arduino ports */

static const unsigned int ROW_PORTS[ROWS] = {
	2, 3, 4
};

static const unsigned int COL_PORTS[COLUMNS] = {
	12, 11, 10, 9, 8
};

static uint32_t irq_counter = 0;
static char x[N];
static char samples[N];

static unsigned int pos = 0;

static byte led_buffer1[ROWS * COLUMNS];
static byte led_buffer2[ROWS * COLUMNS];

static uint32_t band_energies[BANDS_NUM];

static void turn_off_all_leds( void )
{
  unsigned int i = 0;
  for (i = 0; i < ROWS; ++i) {
    pinMode(ROW_PORTS[i], INPUT);
  }
  
  for (i = 0; i < COLUMNS; ++i) {
    pinMode(COL_PORTS[i], INPUT);
  }
} /* end of turn_off_all_leds() */

static void turn_on_single_led(unsigned int row, 
							   unsigned int col)
{
	pinMode(ROW_PORTS[row], OUTPUT);
	pinMode(COL_PORTS[col], OUTPUT); 
	digitalWrite(ROW_PORTS[row], HIGH);
	digitalWrite(COL_PORTS[col], LOW);
}

#define TEST_DELAY         (100)

static void test_leds()
{
        byte* buffer = led_matrix_get_empty_buffer();
	unsigned int i, j;
	
	turn_off_all_leds();
	
	for (i = 0; i < ROWS; ++i) {
		for (j = 0; j < COLUMNS; ++j) {
			turn_on_single_led(i, j);
			delay(TEST_DELAY);
			turn_off_all_leds();
		}
	}

        for (i = 0; i < BANDS_NUM; ++i) {
            band_energies[i] = 500;    
        }
        
        fill_led_buffer(buffer, band_energies, BANDS_NUM);
        led_matrix_draw(1);
        for (i = 0; i < 10; ++i) {
          led_matrix_update();
          delay(TEST_DELAY);
        }
} /* test_leds() */

static void blink()
{
  digitalWrite(BUILTIN_LED_PIN, HIGH);
  delay(500);
  digitalWrite(BUILTIN_LED_PIN, LOW);
}

void setup( void )
{
	pinMode(ANALOG_PIN_OFFSET + MIKE_PIN, INPUT);
        pinMode(BUILTIN_LED_PIN, OUTPUT);
	
        led_matrix_init(led_buffer1, led_buffer2,
					ROWS, COLUMNS,
					turn_on_single_led,
					turn_off_all_leds);
	
        
	test_leds();
        
	/* set timer according to sampling rate */
	Timer1.initialize(USEC_IN_SEC / SAMPLING_RATE);
	
        blink();
        
        //turn_off_all_leds();
        Timer1.attachInterrupt(timerIsr);
} /* end of setup() */

static void calc_band_energies(char samples[])
{
	unsigned int i;
	
	for (i = 0; i < BANDS_NUM; ++i) {
		band_energies[i] = calc_energy(samples, 
									   BAND_RANGE[i][0], 
									   BAND_RANGE[i][1]);
	}
} /* end of calc_band_energies */

static void fill_led_buffer(byte buffer[],
							uint32_t band_enrg[],
							unsigned int bands_num)
{
	unsigned int i, j;
        int enrg;
	unsigned int max_row;
	
	for (i = 0; i < bands_num; ++i) {
		enrg = band_enrg[i] - BAND_THRESHOLDS[i];
		enrg = constrain(enrg, 0, BAND_PEAKS[i]);
		max_row = map(enrg, 0, BAND_PEAKS[i], 0, ROWS);
		
		for (j = 0; j < max_row; ++j) {
			buffer[j * COLUMNS + i] = 1;
		}
	}
} /* end of fill_led_buffer */

void timerIsr( void )
{ 
  int scale = 0;
  int val;
  byte* cur_buffer = NULL;
  unsigned int i;
  
  /* read a new sample */
  val = analogRead(MIKE_PIN);
  x[pos] = map(val, 0, 1023, -127, 128);
  pos = (pos + 1) % N;
  
  if (pos == 0) { 
    for (int i = 0; i < N; ++i) {
      samples[i] = x[(pos + i) % N];
    }
    
    scale = fix_fftr(samples, LOG_2_N, 0);  
    calc_band_energies(samples);
    	
    cur_buffer = led_matrix_get_empty_buffer();
    fill_led_buffer(cur_buffer, band_energies, BANDS_NUM);
    	
    led_matrix_draw( 1 );  
  }
  
  if (pos % (N/2) == 0) {
      led_matrix_update();
  }
  
  ++irq_counter;
  if (irq_counter % SAMPLING_RATE == 0) {
    digitalWrite(BUILTIN_LED_PIN,  
                                   (digitalRead(BUILTIN_LED_PIN) + 1) % 2);
  }
} /* end of timerIsr */

uint32_t calc_energy(char f[], unsigned int start_freq, unsigned int end_freq)
{
  uint32_t energy = 0;

  for (int k = start_freq; k <= end_freq; ++k) {
    energy += f[k] * f[k];
  }

  return energy;
} /* end of calc_energy */

void loop( void )
{
}
